//---------------------------------------------------------------------------

#include <vcl.h>
#pragma hdrstop

#include "SimWriteMotor.h"
#include "GlobalVariables.h"
//---------------------------------------------------------------------------
#pragma package(smart_init)
#pragma resource "*.dfm"
TFormSimWriteMotor *FormSimWriteMotor;
unsigned char TempBuf[TEMPBUF_SIZE] = {0};
LARGE_INTEGER li_CPUFreq = {0};
LARGE_INTEGER li_RecTime = {0};
__int64 i64_CPUFreq = 0;
__int64 i64_RecTime_Past = 0;
__int64 i64_RecTime_Now = 0;
double IntervalTime = 0;

//---------------------------------------------------------------------------
__fastcall TFormSimWriteMotor::TFormSimWriteMotor(TComponent* Owner)
	: TForm(Owner)
{
}
//---------------------------------------------------------------------------
void __fastcall TFormSimWriteMotor::FormCreate(TObject *Sender)
{
	FormSimWriteMotor->Top = 150;
	FormSimWriteMotor->Left = 100;

	QueryPerformanceFrequency(&li_CPUFreq);
	i64_CPUFreq = li_CPUFreq.QuadPart;
}
//---------------------------------------------------------------------------

bool TFormSimWriteMotor::WriteMotor_DataCheck(unsigned char* buf, int length_buf)
{
	int num_nondealed_data = 0;
	static int threshold_num_nondealed_data = 2;
	unsigned char checksum = 0;
	for(int i=0; i < length_buf; i++)
	{
		RXQueue[RX_tail] = buf[i];
		RX_tail = (RX_tail + 1) % RXQUEUE_SIZE;
	}
	if (RX_head <= RX_tail) {
		num_nondealed_data = RX_tail - RX_head;
	}
	else {
		num_nondealed_data = RXQUEUE_SIZE - (RX_head - RX_tail);
	}

	while(num_nondealed_data >= threshold_num_nondealed_data)
	{
		//MemoReceive->Lines->Add(IntToStr(num_nondealed_data) + ", " +IntToStr(threshold_num_nondealed_data));
		QueryPerformanceCounter(&li_RecTime);
		i64_RecTime_Now = li_RecTime.QuadPart;
		IntervalTime = (double)(i64_RecTime_Now - i64_RecTime_Past) / i64_CPUFreq;
    i64_RecTime_Past = i64_RecTime_Now;
		if(CheckBoxShowTime->Checked)
		{
			MemoIntervalTime->Lines->Add(FormatFloat("0.00", IntervalTime*1000)+" ms");
		}

		if(RXQueue[((RX_head+0) % RXQUEUE_SIZE)] == 0xFF &&
			 RXQueue[((RX_head+1) % RXQUEUE_SIZE)] == 0xFF &&
			 RXQueue[((RX_head+2) % RXQUEUE_SIZE)] == 0XFE &&
			 RXQueue[((RX_head+4) % RXQUEUE_SIZE)] == 0x83 &&
			 RXQueue[((RX_head+5) % RXQUEUE_SIZE)] == 0x1E &&
			 RXQueue[((RX_head+6) % RXQUEUE_SIZE)] == 0x04)
		{
			if(threshold_num_nondealed_data == (RXQueue[((RX_head+3) % RXQUEUE_SIZE)]+4))
			{
				int MotorNum = (RXQueue[((RX_head+3) % RXQUEUE_SIZE)]-4) / 5;
				checksum = 0xA3 + RXQueue[((RX_head+3) % RXQUEUE_SIZE)];
				for(int i=0; i < MotorNum*5; i++)
				{
					checksum += RXQueue[((RX_head+7+i) % RXQUEUE_SIZE)];
				}
				checksum = ~checksum;

				if(checksum == RXQueue[((RX_head+7+(MotorNum*5)) % RXQUEUE_SIZE)])
				{
					// Measure the interval time
					for(int i=0; i < MotorNum*5; i++)
					{
						TempBuf[(i%TEMPBUF_SIZE)] = RXQueue[((RX_head+7+i) % RXQUEUE_SIZE)];
					}
					UpdateWriteMotorDataInfo(&TempBuf[0], MotorNum);
					RX_head = 0;
					RX_tail = 0;
					return true;
				}
				else
				{
					RX_head = (RX_head + 1) % RXQUEUE_SIZE;
				}
			}
			else
			{
				threshold_num_nondealed_data = RXQueue[((RX_head+3) % RXQUEUE_SIZE)] + 4;
      }
		}
		else
		{
      RX_head = (RX_head + 1) % RXQUEUE_SIZE;
		}

		if (RX_head <= RX_tail) {
			num_nondealed_data = RX_tail - RX_head;
		}
		else {
			num_nondealed_data = RXQUEUE_SIZE - (RX_head - RX_tail);
		}
	}
	return false;
}
//---------------------------------------------------------------------------

void TFormSimWriteMotor::UpdateWriteMotorDataInfo(unsigned char* MotorData, int MotorNum)
{
	int Position = 0;
	int Speed = 0;
	int ID = 0;
	int index = 0;
	for(int i=0; i < MotorNum; i++)
	{
		ID = MotorData[(index%TEMPBUF_SIZE)];
		Position = MotorData[(index+1)%TEMPBUF_SIZE] + (MotorData[(index+2)%TEMPBUF_SIZE] << 8);
		Speed = MotorData[(index+3)%TEMPBUF_SIZE] + (MotorData[(index+4)%TEMPBUF_SIZE] << 8);
		if(CheckBoxShow->Checked)
		{
			if(Position > 4095)
			{
				MemoReceive->Lines->Add(">> ID: " + IntToStr(ID)+
																", Pos>4095: " + IntToStr(Position-4095) +
																", Speed: " + IntToStr(Speed));
			}
			else
			{
				MemoReceive->Lines->Add(">> ID: " + IntToStr(ID)+
																", Pos: " + IntToStr(Position) +
																", Speed: " + IntToStr(Speed));
			}
		}
		index += 5;
  }
}
//---------------------------------------------------------------------------


void __fastcall TFormSimWriteMotor::MemoReceiveDblClick(TObject *Sender)
{
  MemoReceive->Text = "";
}
//---------------------------------------------------------------------------

void __fastcall TFormSimWriteMotor::MemoIntervalTimeDblClick(TObject *Sender)
{
	MemoIntervalTime->Text = "";
}
//---------------------------------------------------------------------------

